﻿// Visentech_Pcl.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include "VisionBooster.h"
#include <vtkOutputWindow.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <tchar.h>
#include <vector>
#include "windows.h"

using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//简单类型定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
//这是一个辅助教程，因此我们可以负担全局变量
//创建可视化工具
pcl::visualization::PCLVisualizer *p;
//定义左右视点
int vp;
#define CAPTURE_TIME_OUT 10000  // 获取点云超时
const char* install_path = "C:\\code\\VS2017\\Falcon\\x64\\Release\\VisenTOP Studio.exe";
using namespace std;

void WINAPI CaptureThreadFunc(LPVOID lpParameter)
{
	int* pResult = (int*)lpParameter;
	*pResult = VST3D_Scan();
}

int Capture3DData()
{
	HANDLE hThread;
	DWORD ThreadID;
	int VST3D_RESULT = VST3D_RESULT_OK;
	hThread = CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)CaptureThreadFunc, &VST3D_RESULT, 0, &ThreadID);
	DWORD hResult = WaitForSingleObject(hThread, CAPTURE_TIME_OUT);//等待6秒,超时报错退出。
	if (hResult == WAIT_OBJECT_0)
	{
		return VST3D_RESULT;
	}
	else if (hResult == WAIT_TIMEOUT)
	{
		return VST3D_RESULT_ERROR; //超时		
	}
	else
	{
		return VST3D_RESULT_ERROR;
	}
}
int waitonce = 0;
void showClouds(const PointCloud::Ptr cloud_target)
{

	
	p->removePointCloud("VST3D");
	PointCloudColorHandlerCustom<PointT> tgt_h(cloud_target, 0, 255, 0);
	p->addPointCloud(cloud_target, tgt_h, "VST3D", vp);
	
	if (waitonce == 0)
	{
		PCL_INFO("Press q to begin the registration.\n");
		p->spin();
		waitonce = 1;
	}
	else
	{
		p->spinOnce();
	}
}

int main(int argc, char** argv)
{
	vtkOutputWindow::SetGlobalWarningDisplay(0);
	int result = VST3D_Init(); //      缺省安装目录启动，正常窗口启动
	 //int result = VST3D_Init(install_path, true);  //缺省安装目录启动,隐藏软件窗口
	if (result != VST3D_RESULT_OK)
	{
		printf("Could not Start Scanner software.\n");
		VST3D_Exit();
		return -1;
	}

	result = VST3D_Connect();     // 正常单机扫描，连上扫描仪并进行自检
	if (result != VST3D_RESULT_OK)
	{
		printf("Could not connect to Scanner.\n");
		VST3D_Exit();
		return -1;
	}

	int N = 0;
	int lossN = 0;
	int totalGroup = 100;     // 设置连续扫描的次数
	vector<int> vGroupNumRecord(totalGroup);
	//初始化PCL显示
	p = new pcl::visualization::PCLVisualizer(argc, argv, "VST3D");
	p->setSize(500, 500);
	p->createViewPort(0, 0, 1, 1, vp);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // 创建点云（指针）
	while (N < totalGroup)
	{
		//result = VST3D_Scan();    //  开始单次扫描
		result = Capture3DData();   //  开始多线程单次扫描，设置超时
		if (result != VST3D_RESULT_OK)
		{
			printf("Scan error...\n");
			int nreset = 0;
			while (true)
			{
				Sleep(CAPTURE_TIME_OUT); // 每次重新连接前延时几秒，相机，光栅硬件驱动加载需要时间

				// 单机扫描情况
				result = VST3D_Reset(install_path); // 单机扫描重新连接扫描仪系统，重启

				if (result != VST3D_RESULT_OK)
				{
					printf("Check cables connected to Scanner.\n");  // 重新连接失败，继续尝试
				}
				else
				{
					break;   //重新连接成功，重新扫描
				}
				if (nreset++ > 5)  // 做5次重新连接尝试，如果不能正常连接扫描仪，退出软件系统
				{
					VST3D_Exit();
					return result;
				}
			}
		}
		
		int totalNum = 0;
		result = VST3D_GetNumPoints(totalNum);  // 得到当前单次扫描总点数
		(*cloud).points.resize(totalNum);	
		if (result != VST3D_RESULT_OK)
		{
			lossN++;
			printf("Frame loss in 3D acquisition, try it again...\n");
		}
		else
		{
			/////////////////////////获得乱序点云///////////////////////////////////////
			int totalNum = 0;
			VST3D_PT* pPointClouds = nullptr;
			result = VST3D_GetPointClouds(totalNum, &pPointClouds);  // 得到当前单次扫描总点数和乱序的点云
			vGroupNumRecord[N] = totalNum;
			for (int i = 0; i < totalNum; i++)    // 遍历单次采集到的所有乱序点云，得到点云坐标等信息，通过索引方式[0 1 2 ....]
			{
				VST3D_PT* pt = nullptr;
				VST3D_GetEachPointByIndex(i, &pt);
				float x, y, z;
				(*cloud).points[i].x = (pt->x)*0.8;
				(*cloud).points[i].y = (pt->y)*0.8;
				(*cloud).points[i].z = (pt->z)*0.3;	
			}	
		}
		showClouds(cloud);
		pcl::io::savePLYFileASCII("point3d.ply", *cloud);
		(*cloud).points.clear();
		cout << "# " << N << " total num is :" << totalNum << endl;
		N++;
	}
	cout << endl;
	cout << "# Total Loss Frame Num is :" << lossN << endl;

	VST3D_Exit();	// 退出扫描操作，卸载设备软件
	system("pause");

	return 0;
}

// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门使用技巧: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
